/*
 * Created Date: Wednesday, September 15th 2021, 4:38:27 pm
 * Author: Han Hu
 * 定义一些基础函数
 * Copyright (c) 2021 Vrlab
 */
#include "common.h"

#include <Eigen/Geometry>

namespace pcv {
Matrix3d rodrigues(const Vector3d &r) {
    double angle = r.norm();
    Vector3d axis = r.normalized();

    Eigen::AngleAxisd aa(angle, axis);

    return aa.toRotationMatrix();
}

Vector3d rodrigues(const Matrix3d &R) {
    Eigen::AngleAxisd aa(R);

    Vector3d axis = aa.axis();
    double angle = aa.angle();
    return angle * axis;
}

Matrix34d P_from_KRt(const Matrix3d &K, const Matrix3d &R, const Vector3d &t) {
    Matrix34d P;
    P << R, t;
    P = K * P;
    return P;
}

std::tuple<Matrix3d, Matrix3d, Vector3d> KRt_from_P(const Matrix34d &P) {
    // Decompose using the RQ decomposition HZ A4.1.1 pag.579.
    Matrix3d K = P.block(0, 0, 3, 3);

    Matrix3d Q;
    Q.setIdentity();

    // Set K(2,1) to zero.
    if (K(2, 1) != 0) {
        double c = -K(2, 2);
        double s = K(2, 1);
        double l = std::hypot(c, s);
        c /= l;
        s /= l;
        Matrix3d Qx;
        Qx << 1, 0, 0, 0, c, -s, 0, s, c;
        K = K * Qx;
        Q = Qx.transpose() * Q;
    }
    // Set K(2,0) to zero.
    if (K(2, 0) != 0) {
        double c = K(2, 2);
        double s = K(2, 0);
        double l = std::hypot(c, s);
        c /= l;
        s /= l;
        Matrix3d Qy;
        Qy << c, 0, s, 0, 1, 0, -s, 0, c;
        K = K * Qy;
        Q = Qy.transpose() * Q;
    }
    // Set K(1,0) to zero.
    if (K(1, 0) != 0) {
        double c = -K(1, 1);
        double s = K(1, 0);
        double l = std::hypot(c, s);
        c /= l;
        s /= l;
        Matrix3d Qz;
        Qz << c, -s, 0, s, c, 0, 0, 0, 1;
        K = K * Qz;
        Q = Qz.transpose() * Q;
    }

    Matrix3d R = Q;

    // Mat3 H = P.block(0, 0, 3, 3);
    // RQ decomposition
    // Eigen::HouseholderQR<Mat3> qr(H);
    // Mat3 K = qr.matrixQR().triangularView<Eigen::Upper>();
    // Mat3 R = qr.householderQ();

    // Ensure that the diagonal is positive and R determinant == 1.
    if (K(2, 2) < 0) {
        K = -K;
        R = -R;
    }
    if (K(1, 1) < 0) {
        Matrix3d S;
        S << 1, 0, 0, 0, -1, 0, 0, 0, 1;
        K = K * S;
        R = S * R;
    }
    if (K(0, 0) < 0) {
        Matrix3d S;
        S << -1, 0, 0, 0, 1, 0, 0, 0, 1;
        K = K * S;
        R = S * R;
    }

    // Compute translation.
    Eigen::PartialPivLU<Matrix3d> lu(K);
    Vector3d t = lu.solve(P.col(3));

    if (R.determinant() < 0) {
        R = -R;
        t = -t;
    }

    // scale K so that K(2,2) = 1
    K = K / K(2, 2);

    return std::make_tuple(K, R, t);
}

} // namespace pcv